#!/usr/bin/env python
import pygame
from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *
from pygame.locals import *
from threading import Thread
import time
from math import *
from numpy import *

#resize gl render window
def resize((width, height)):
	if height==0:
		height=1.0
	glViewport(0, 0, width, height)
	glMatrixMode(GL_PROJECTION)
	glLoadIdentity()
	gluPerspective(45, 1.0*width/height, 0.1, 50000.0)
	glMatrixMode(GL_MODELVIEW)
	glLoadIdentity()

#initialize gl environment
def InitGL():

	#light properties
	glShadeModel(GL_SMOOTH)
    	glClearColor(0.6, 0.6, 1.0, 0.0)
    	glClearDepth(1.0)
    	glEnable(GL_DEPTH_TEST)
    	glDepthFunc(GL_LEQUAL)
    	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST)
	glEnable(GL_NORMALIZE);
	glDisable(GL_LIGHTING)
	glEnable(GL_LIGHTING)
	glLightfv( GL_LIGHT1, GL_AMBIENT, (0.2,0.2,0.2,1.0) )
	glLightfv( GL_LIGHT1, GL_DIFFUSE, (0.0,0.0,0.0,1.0) )
	glLightfv( GL_LIGHT1, GL_SPECULAR,(0.0,0.0,0.0,1.0) )
	glLightfv( GL_LIGHT1, GL_POSITION,(0.0,0.0,0.0,1.0))
	glEnable( GL_LIGHT1 )
	glLightfv( GL_LIGHT2, GL_AMBIENT, (0.0,0.0,0.0,1.0) )
	glLightfv( GL_LIGHT2, GL_DIFFUSE, (0.5,0.5,0.5,1.0) )
	glLightfv( GL_LIGHT2, GL_SPECULAR,(0.5,0.5,0.5,1.0) )
	glLightfv( GL_LIGHT2, GL_POSITION,(1000,-1000,1500) )
	glEnable( GL_LIGHT2 )
	glLightfv( GL_LIGHT3, GL_AMBIENT, (0.0,0.0,0.0,1.0) )
	glLightfv( GL_LIGHT3, GL_DIFFUSE, (0.5,0.5,0.5,1.0) )
	glLightfv( GL_LIGHT3, GL_SPECULAR,(0.5,0.5,0.5,1.0) )
	glLightfv( GL_LIGHT3, GL_SPOT_DIRECTION,(0,0,1) )
	glLightfv( GL_LIGHT3, GL_SPOT_CUTOFF,40 )
	glLightfv( GL_LIGHT3, GL_POSITION,(0,0,-1000,1.0) )
	glEnable( GL_LIGHT3 )
	glEnable(GL_COLOR_MATERIAL)
	#Set default material
	glMaterialfv(GL_FRONT_AND_BACK , GL_SPECULAR , (1.0,1.0,1.0,1.0)) 
	glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, 100.0)
	glMaterialfv(GL_FRONT_AND_BACK , GL_DIFFUSE , (1.0,1.0,1.0,1.0)) 
	glutInit()

def drawBox(position,rotation,size=(10.0,10.0,10.0),color=(1.0,1.0,1.0)):
	x,y,z=position
	glPushMatrix()
	glTranslate(x,y,z)
	roll,pitch,yaw=rotation
	glRotate(roll,1.0,0.0,0.0)
	glRotate(pitch,0.0,1.0,0.0)
	glRotate(yaw,0.0,0.0,1.0)
	glScale(size[0], size[1], size[2])
	glColor3f(color[0],color[1],color[2])
	glutSolidCube(1)
	glPopMatrix()

def drawSphere(position,rotation,size=10,color=(1.0,1.0,1.0),slices=20,stacks=20):
	x,y,z=position
	glPushMatrix()
	glTranslate(x,y,z)
	roll,pitch,yaw=rotation
	glRotate(roll,1.0,0.0,0.0)
	glRotate(pitch,0.0,1.0,0.0)
	glRotate(yaw,0.0,0.0,1.0)
	glColor3f(color[0],color[1],color[2])
	glutSolidSphere(size,slices,stacks)
	glPopMatrix()

def drawCylinder(position,rotation,radius=10,height=10,color=(1.0,1.0,1.0),slices=20,stacks=20):
	x,y,z=position
	z-=float(height)/2.0
	glPushMatrix()
	glTranslate(x,y,z)
	roll,pitch,yaw=rotation
	glRotate(roll,1.0,0.0,0.0)
	glRotate(pitch,0.0,1.0,0.0)
	glRotate(yaw,0.0,0.0,1.0)
	glColor3f(color[0],color[1],color[2])
	glutSolidCylinder(radius,height,slices,stacks)
	glPopMatrix()

def drawPlane(sides=(1.0,1.0),normal=(0.0,0.0,1.0),up=(0.0,1.0,0.0),position=(0.0,0.0,0.0), color=(0.1,0.3,0.1)):
	r,g,b=color	
	glColor3fv(color)
	glNormal3fv(normal)
	right=cross(array(up),array(normal))

	BBT= [   right[0],    right[1],    right[2], 0., 
		    up[0],       up[1],       up[2], 0.,
		normal[0],   normal[1],   normal[2], 0.,
	      position[0], position[1], position[2], 1.0]
	glPushMatrix()
	glMultMatrixf(BBT)
	glRectfv((-sides[0]/2,-sides[1]/2),(sides[0]/2,sides[1]/2))
    	glPopMatrix()


class VisThread(Thread):
	def __init__(self,links,resolution,fps=200):
		Thread.__init__(self)
		self.links=links
		self.running=True
		self.clock=pygame.time.Clock()
		self.fps=fps
		self.resolution=resolution

		
		

	def run(self):

		#init pygame
		pygame.init()
		pygame.font.init()
		print("pygame initialized")
		#set pygame screen size and properties
		screen = pygame.display.set_mode(self.resolution, HWSURFACE|OPENGL|DOUBLEBUF|OPENGLBLIT)
		#initialize GL
		InitGL()
		resize(self.resolution)


		#start cam steady
		rotate = move = False


		rotlr=0.0
		rotud=90.0
		movelr=0.0
		moveud=0.0

		camdist=700.0

		cam=array([0.0,0.0,0.0])
		camt=array([0.0,0.0,0.0])
		camup=array([0.0,0.0,-1.0])


		print("Visualization Thread Running")
		while self.running:
			
			#pygame clock tick. limits framerate to "fps"
			self.clock.tick(self.fps)
			#get average fps
			avg_fps=self.clock.get_fps()
			#print("Average FPS: "+str(avg_fps))

			#handle keys				
			for e in pygame.event.get():
				if e.type == QUIT:
					self.running=False
					break

				elif e.type == KEYDOWN:
					if e.key == K_ESCAPE:
						self.running=False
						break
					elif e.key==K_c:
						camt=zeros(3)
						rotlr=0
						rotud=90
				elif e.type == MOUSEBUTTONDOWN:
					if e.button == 4: 
						#zoom in
						camdist-=0.1*camdist
					elif e.button == 5: 
						#zoom out
						camdist+=0.1*camdist
					elif e.button == 1: 
						rotate = True
					elif e.button == 3: 
						move = True
				elif e.type == MOUSEBUTTONUP:
					if e.button == 1: 
						rotate = False
					elif e.button == 3: 
						move = False
				elif e.type == MOUSEMOTION:
					i, j = e.rel
					if rotate:
						rotlr += i
						rotud += j
					if move:
						movelr = i
						moveud = j


			# Clear The Screen And The Depth Buffer
			glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
			# Reset Matrix
			glLoadIdentity()
			
			#limit cam up down movement
			if rotud<0:
				rotud=1e-14
			if rotud>180:
				rotud=180.0

			#first calculate cam pos from angles and distance to target
			if rotlr==0 and rotud==0:
				cam[0]=camt[0]
				cam[1]=camt[1]
				cam[2]=camdist+camt[2]
			elif rotlr==0:
				cam[0]=camdist+camt[0]
				cam[1]=camt[1]
				cam[2]=camdist*cos(radians(rotud))+camt[2]
			elif rotud==0:
				cam[0]=camt[0]
				cam[1]=camt[1]
				cam[2]=camdist+camt[2]
			else:
				cam[0]=camdist*sin(radians(rotud))*cos(radians(rotlr))+camt[0]
				cam[1]=camdist*sin(radians(rotud))*sin(radians(rotlr))+camt[1]
				cam[2]=camdist*cos(radians(rotud))+camt[2]
		
			#calculate vector pointing sideways from cam			
			camside=cross(cam-camt,camup)
			camside=camside/sqrt(dot(camside,camside.conj()))
			#camculate vector pointing up from cam
			camupg=cross(camside,cam)
			camupg=camupg/sqrt(dot(camupg,camupg.conj()))
			#move cam pos and cam target pos in movement directions
			cam+=camside*movelr+camupg*moveud
			camt+=camside*movelr+camupg*moveud

			#call opengl cam placement function
			gluLookAt(cam[0],cam[1],cam[2],camt[0],camt[1],camt[2],camup[0],camup[1],camup[2])
			#zero movement vars
			movelr=0.0
			moveud=0.0
			glLightfv( GL_LIGHT1, GL_POSITION,(0,0,-1500) )
			glLightfv( GL_LIGHT2, GL_POSITION,(1000,-1000,1000) )
			glLightfv( GL_LIGHT3, GL_POSITION,(1000,1000,-2000,1.0) )
			glLightfv( GL_LIGHT3, GL_SPOT_DIRECTION,(-1,-1,2) )

			#drawBox((0,0,0),(0,45,45),(50,50,50))
			drawSphere((0.0,0.0,0.0),(0.0,0.0,0.0),5,(1,0,0),32,32)
			#drawCylinder((0,0,0),(0,0,0),100,20,(1,1,1),100,100)
			drawPlane(sides=(1000000,1000000),normal=(0.0,0.0,1.0),up=(0.0,1.0,0.0),position=(0.0,0.0,1500), color=(0,0.7,0))
			#tstart=pygame.time.get_ticks()

			#draw all links.
			for link in self.links:
				link.draw()
				pass

			

			#tend=pygame.time.get_ticks()
			#print tend-tstart

			pygame.display.set_caption("Robot Visualizer | Press Escape to quit | Press ""c"" to center view | FPS: "+str(int(avg_fps)))
			#update pygame display
			pygame.display.flip()



